home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / demos / GL / newton / physics.c < prev    next >
Encoding:
C/C++ Source or Header  |  1994-08-02  |  9.3 KB  |  446 lines
  1. /*
  2.  * Copyright 1991, 1992, 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. /*
  18.  * physics calculations of the model motion
  19.  * Yossi Friedman, July 1988
  20.  */
  21.  
  22. #include <stdio.h>
  23. #include <gl.h>
  24. #include <math.h>
  25.  
  26. #include "config.h"
  27. #include "newton.h"
  28. #include "slider.h"
  29.  
  30. /* free physical parameters */
  31. float gravity;            /* current gravity */
  32. float wall_k;            /* spring constant of the walls */
  33. float fric;            /* friction of the wall */
  34. float air_damp;            /* air friction */
  35. float disp_step;        /* display time increment */
  36.  
  37.  
  38.  
  39. /* dependent physical parameters */
  40. static float dt;        /* computation time increment */
  41. static float t;                   /* current time, modulo dt */
  42.  
  43.  
  44. float grav_vec_V[3];        /* real gravity vector (in viewing system) */
  45. float grav_vec[3];        /* gravity vector in modeling coord system */
  46.  
  47.  
  48. struct slider *k_slider;    /* ptr. to K slider */
  49.  
  50. void new_gravity(float),  new_k(float),
  51.      new_wall_k(float),   new_fric(float),
  52.      new_air_damp(float), new_disp_step(float);
  53.  
  54. initialize_physics()
  55. {
  56.     /* gravity vector should point down in the viewing coordinate system */
  57.     grav_vec_V[X] = grav_vec_V[Z] = 0.;
  58.  
  59.     /* set the sliders properly */
  60.     initialize_slider_parameters();
  61.     t = 0.;
  62.  
  63.     stop_gravity();
  64. }
  65.  
  66.  
  67. initialize_slider_parameters()
  68. {
  69.     struct slider *sp;
  70.  
  71.     sp = sliders;
  72.  
  73.     sp->title = "Gravity";
  74.     sp->lo  = 0.;
  75.     sp->hi  = 0.4;
  76.     sp->dflt = 0.1;
  77.     sp->fun = new_gravity;
  78.  
  79.     sp++;
  80.  
  81.     sp->title = "Spring Constant";
  82.     sp->lo  = 0.1;
  83.     sp->hi  = 50.;
  84.     /* sp->dflt = max_k; */
  85.     sp->dflt = 0.;        /* dummy */
  86.     sp->fun = new_k;
  87.     k_slider = sp;
  88.  
  89.     sp++;
  90.  
  91.     sp->title = "Wall Stiffness";
  92.     sp->lo  = 0.01;
  93. #if ECLIPSE || CLOVER1        /* the slow machines */
  94.     sp->hi  = 0.8;
  95.     sp->dflt = 0.3;
  96. #else /* ECLIPSE || CLOVER1 */
  97.     sp->hi  = 1.5;
  98.     sp->dflt = 0.8;
  99. #endif /* ECLIPSE || CLOVER1 */
  100.     sp->fun = new_wall_k;
  101.  
  102.     sp++;
  103.  
  104.     sp->title = "Wall Friction";
  105.     sp->lo  = 1. - 1.;
  106.     sp->hi  = 1. - 0.5;
  107.     sp->dflt = 1. - 0.9;
  108.     sp->fun = new_fric;
  109.  
  110.     sp++;
  111.  
  112.     sp->title = "Air Dampening";
  113.     sp->lo  = 1. - 1.;
  114.     sp->hi  = 1. - 0.99;
  115.     sp->dflt = 1. - 0.9995;
  116.     sp->fun = new_air_damp;
  117.  
  118.     sp++;
  119.  
  120.     sp->title = "Display Step";
  121.     sp->lo  = 1.;
  122.     sp->hi  = 7.;
  123. #if ECLIPSE || CLOVER1        /* the slow machines */
  124.     sp->dflt = 6.5;
  125. #else /* ECLIPSE || CLOVER1 */
  126.     sp->dflt = 3.0;
  127. #endif /* ECLIPSE || CLOVER1 */
  128.     sp->fun = new_disp_step;
  129.  
  130.     sp++;
  131.  
  132.     end_sliders = sp;
  133. }
  134.  
  135.  
  136. void
  137. new_gravity(float val)
  138. {
  139.     gravity = val;
  140.     if (grav_vec_V[Y] != 0.) {
  141.     grav_vec_V[Y] = gravity;
  142.  
  143.     /* get the gravity vector to point down properly */
  144.     apply(grav_vec, grav_vec_V, M_inv);
  145.     }
  146. }
  147.  
  148. void
  149. new_k(float val)
  150. {
  151.     Spring *spring;
  152.     float k;
  153.  
  154.     if (val == 0.)     /* dummy */
  155.     return;
  156.  
  157.     for (spring = model_springs; spring < end_model_springs TOTAL; spring++)
  158.     spring->k *= val/max_k;
  159.  
  160.     max_k = val;
  161.     
  162.     /* calculate dt */
  163.     k = (max_k > wall_k)? max_k: wall_k;
  164.     dt = sqrt(2./k) * 0.5;        /* fifty percent thumb rule */
  165. }
  166.  
  167. void
  168. new_wall_k(float val)
  169. {
  170.     wall_k = val;
  171. }
  172.     
  173. void
  174. new_fric(float val)
  175. {
  176.     fric = 1. - val;
  177. }
  178.     
  179. void
  180. new_air_damp(float val)
  181. {
  182.     air_damp = 1. - val;
  183. }
  184.  
  185. void
  186. new_disp_step(float val)
  187. {
  188.     disp_step = val;
  189. }
  190.  
  191.  
  192. stop_gravity()
  193. {
  194.     grav_vec[X] =
  195.     grav_vec[Y] =
  196.     grav_vec[Z] =
  197.     grav_vec_V[X] =
  198.     grav_vec_V[Y] =
  199.     grav_vec_V[Z] = 0.0;
  200.  
  201. }
  202.  
  203. start_gravity()
  204. {
  205.     grav_vec_V[Y] = gravity;
  206.  
  207.     /* get the gravity vector to point down properly */
  208.     apply(grav_vec, grav_vec_V, M_inv);
  209. }
  210.  
  211.  
  212. void
  213. set_default_k(float k)
  214. {
  215.     set_slider_default(k_slider->sid, k);
  216.     set_k(k);
  217. }
  218.  
  219.  
  220. void
  221. set_k(float k)
  222. {
  223.     set_slider(k_slider->sid, k);
  224. }
  225.  
  226.  
  227. /*
  228.  * rotate the physics (gravity and model atoms vectors) in the ROOM
  229.  * coordinate system angle alpha around a VIEWING axis
  230.  */
  231. rotate_physics(Angle alpha, char axis)
  232. {
  233.     /* first get the gravity vector to point down properly */
  234.     apply(grav_vec, grav_vec_V, M_inv);
  235.     
  236.     /*
  237.      * the formula is:
  238.      *   v R = v M rot M_inv,
  239.      * where rot is the rotation matrix (in the viewing system) around the
  240.      * viewing axis.
  241.      */
  242.     pushmatrix();
  243.       loadmatrix(M_inv);
  244.       rotate(alpha, axis);
  245.       multmatrix(M);
  246.       getmatrix(R);
  247.     popmatrix();
  248.  
  249.     SLAVE_FUNC(DO_ROTATE_PHYSICS);
  250.     do_rotate_physics(model_atoms, end_model_atoms MASTER);
  251.     WAIT_FOR_SLAVE();
  252. }
  253.  
  254.  
  255. do_rotate_physics(Atom *start, Atom *finish)
  256. {
  257.     Atom *ap;
  258.     float ov[3], *v;
  259.     
  260.     for (ap = start; ap < finish; ap++) {
  261.     v = ap->pos;
  262.     ov[X] = v[X]; ov[Y] = v[Y]; ov[Z] = v[Z];
  263.     apply(v, ov, R);
  264.  
  265.     v = ap->vel;
  266.     ov[X] = v[X]; ov[Y] = v[Y]; ov[Z] = v[Z];
  267.     apply(v, ov, R);
  268.     }
  269. }
  270.  
  271.  
  272. iterate()
  273. {
  274.     if(dt <= 0.0)
  275.     return;
  276.  
  277.     for (; t < disp_step; t += dt) {
  278.     SLAVE_FUNC(DO_CLEAR);
  279.         do_clear(model_atoms, end_model_atoms MASTER);
  280.     WAIT_FOR_SLAVE();
  281.  
  282.     wall[0].penetrated = wall[1].penetrated = wall[2].penetrated = 
  283.         wall[3].penetrated = wall[4].penetrated = wall[5].penetrated = 0;
  284.  
  285.     wall[0].depth = wall[1].depth = wall[2].depth = 
  286.         wall[3].depth = wall[4].depth = wall[5].depth = 0;
  287.  
  288.     SLAVE_FUNC(DO_ACCEL);
  289.     do_accel(model_springs, end_model_springs MASTER MASTER_PARAM);
  290.     WAIT_FOR_SLAVE();
  291.  
  292.     SLAVE_FUNC(DO_BOUNDS);
  293.         do_bounds(model_atoms, end_model_atoms MASTER);
  294.     WAIT_FOR_SLAVE();
  295.  
  296.     SLAVE_FUNC(DO_VEL_DAMP_POS);
  297.         do_vel_damp_pos(model_atoms, end_model_atoms MASTER);
  298.     WAIT_FOR_SLAVE();
  299.     }
  300.  
  301.     t -= disp_step;
  302. }
  303.  
  304.  
  305. do_clear(Atom *start, Atom *finish)
  306. {
  307.     Atom *ap;
  308.  
  309.     for(ap = start; ap < finish; ap++) {
  310.     ap->acc MASTER [X] = -grav_vec[X] * dt;
  311.     ap->acc MASTER [Y] = -grav_vec[Y] * dt;
  312.     ap->acc MASTER [Z] = -grav_vec[Z] * dt;
  313.  
  314. #ifdef MP
  315.  
  316.     {
  317.         int i;
  318.  
  319.         for (i = 1; i < nproc; i++) {
  320.         ap->acc [i] [X] = 
  321.         ap->acc [i] [Y] = 
  322.         ap->acc [i] [Z] = 0.;
  323.         }
  324.     }
  325.  
  326. #endif /* MP */
  327.  
  328.     }
  329. }
  330.  
  331.  
  332. do_accel(Spring *start, Spring *finish, CPU_PARAM_TYPE)
  333. {
  334.     Spring *spring;
  335.     float r[3], lri, a[3], F;
  336.     
  337.     for (spring = start; spring < finish; spring++) {
  338.     vec_op(r, spring->from->pos, -, spring->to->pos);
  339.     
  340.     lri = 1. / sqrt(r[X]*r[X] + r[Y]*r[Y] + r[Z]*r[Z]);    
  341.     r[X] *= lri;
  342.     r[Y] *= lri;
  343.     r[Z] *= lri;
  344.     
  345.     F = spring->k * (1. - spring->r0*lri) * 0.5;
  346.     /* the division by 2 above is because half the force is applied
  347.        on each endpoint */
  348.         
  349.     a[X] = r[X] * F;
  350.     a[Y] = r[Y] * F;
  351.     a[Z] = r[Z] * F;
  352.  
  353.     vec_op(spring->from->acc CPU, spring->from->acc CPU, -, a);
  354.     vec_op(spring->to->acc CPU, spring->to->acc CPU, +, a);
  355.     }
  356. }
  357.  
  358.  
  359. do_bounds(Atom *start, Atom *finish)
  360. {
  361.     Atom *ap;
  362.  
  363. /*
  364.  * do_wall is a tricky code that checks weather an atom penetrated a wall.
  365.  * if it did, do_dent applies a force on the atom with a direction back into
  366.  * the room and a magnitude proportional to the penetration depth (i.e., the
  367.  * wall springs are linear).  It also calls do_dent to record the amount of
  368.  * dent in the wall, for when the wall is drawn.
  369.  *
  370.  * The room is the cube { x \in [-HEIGHT,+HEIGHT]^3 }.
  371.  *
  372.  * If you absolutely have to go throught this code, re-write it manually
  373.  * for a specific wall, and notice the symmetry conditions implied for all
  374.  * walls.  Note that if the wall is on AXIS, the other two axises can
  375.  * be obtained by (AXIS+1)%3 and (AXIS+2)%3.
  376.  */
  377. #define do_wall(WALL, AXIS, sgn)                    \
  378.     /* int WALL; int AXIS; sign sgn; */                    \
  379. do {                                    \
  380.     float depth = 0. sgn (ap->pos[AXIS] - (0. sgn HEIGHT));        \
  381.                                                                     \
  382.     if (depth > 0.) {                            \
  383.                                     \
  384.     /* apply forces from wall */                    \
  385.     ap->acc MASTER [AXIS] -= wall_k * (0. sgn depth);        \
  386.     ap->vel[(AXIS+1)%3] *= fric;                    \
  387.     ap->vel[(AXIS+2)%3] *= fric;                    \
  388.                                                                     \
  389.     /* record deepest penetration position */            \
  390.     if ((depth - wall[WALL].depth) >                \
  391.          (TOLERANCE * TOLERANCE - 1.) *                \
  392.          HEIGHT                            \
  393.         ) {                                \
  394.         wall[WALL].penetrated = 1;                    \
  395.         wall[WALL].atom = ap;                    \
  396.                                                                 \
  397.         wall[WALL].depth = depth;                    \
  398.     }                                \
  399.     }                                    \
  400. } while (0)
  401.  
  402.     for (ap = start; ap < finish; ap++) {
  403.     do_wall(WALL_BOTTOM, Y, -);
  404.     do_wall(WALL_TOP,    Y, +);
  405.     do_wall(WALL_LEFT,   X, -);
  406.     do_wall(WALL_RIGHT,  X, +);
  407.     do_wall(WALL_BACK,   Z, -);
  408.     do_wall(WALL_FRONT,  Z, +);
  409.     }
  410.  
  411. #undef do_wall
  412.  
  413. }
  414.  
  415.     
  416. do_vel_damp_pos(Atom *start, Atom *finish)
  417. {
  418.     Atom *ap;
  419.  
  420.     for (ap = start; ap < finish; ap++) {
  421.  
  422. #ifdef MP
  423.  
  424.     {
  425.         int i;
  426.  
  427.         for (i = 1; i < nproc; i++)
  428.             vec_op(ap->acc MASTER, ap->acc MASTER, +, ap->acc[i]);
  429.     }
  430.  
  431. #endif /* MP */
  432.  
  433.     /* calculate the velocity */
  434.  
  435.     vec_op(ap->vel, ap->vel, +, dt * ap->acc MASTER);
  436.  
  437.     /* calculate air dampening */
  438.     ap->vel[X] *= air_damp;
  439.     ap->vel[Y] *= air_damp;
  440.     ap->vel[Z] *= air_damp;
  441.  
  442.     /* calculate position */
  443.     vec_op(ap->pos, ap->pos, +, dt * ap->vel);
  444.     }
  445. }
  446.